#include "WPILib.h"
#include "Constants.h"
#include "Shooter.h"
#include "Basket.h"
#include "ShooterEncoder.h"
#include <string>
#include <cstdlib>
#include <cmath>

const float Shooter::RPM_PER_MPS = 1.0;

Shooter::Shooter(
	int flywheelChannel1,
	int flywheelChannel2,
	int irbreakChannel
)
{
	flywheel = new MatedVictor(
		new Victor(flywheelChannel1), 
		new Victor(flywheelChannel2)
	);
	
	irbreak = new ShooterEncoder(irbreakChannel);
	flywheelPID = new PIDController(
		0.15, 
		0.002, 
		0.002, 
		irbreak, 
		flywheel
		
		/*0.15, 
		0.002, 
		0.001, */
	);
	flywheelPID->SetInputRange(0, 100.0);
	flywheelPID->SetOutputRange(0.0, 1.0);
	flywheelPID->SetSetpoint(0.0);

	//flywheelPID->SetTolerance(1.0);
	flywheelPID->SetTolerance(2.0);
		
	dashboard = SmartDashboard::GetInstance();
	
	dashboard->PutString("Flywheel P", "0.15");
	dashboard->PutString("Flywheel I", "0.002");
	dashboard->PutString("Flywheel D", "0.001");
}

Shooter::~Shooter()
{
	delete flywheel;
	delete flywheelPID;
	delete irbreak;
}

void Shooter::EnablePIDs(bool value)
{
	if (value)
	{
		flywheelPID->Reset();
		flywheelPID->Enable();
	}
	else
	{
		flywheelPID->Reset();
		flywheelPID->Disable();
	}
}

float Shooter::GetSpeed()
{
	return irbreak->GetAverageRPS();
}

void Shooter::SetRawSpeed(float value)
{
	if (!flywheelPID->IsEnabled())
		flywheel->Set(value);
}

void Shooter::SetSpeed(float value)
{
	flywheelPID->SetSetpoint(value * RPM_PER_MPS);
}

bool Shooter::UpToSpeed()
{
	return flywheelPID->OnTarget();
}

float Shooter::GetTargetSpeed()
{
	return flywheelPID->GetSetpoint();
}

float Shooter::ShootTo(float distance)
{
	if(distance < 10.0)
	{
		return 33;
	}
	else
	{
	return .0412 * 	pow(distance,3) - 1.781 * pow(distance,2) + 26.113 * distance - 91.352;
	}
}

void Shooter::Update()
{
	flywheelPID->SetPID(
		atof(dashboard->GetString("Flywheel P").c_str()), 
		atof(dashboard->GetString("Flywheel I").c_str()), 
		atof(dashboard->GetString("Flywheel D").c_str())
	);
	
	irbreak->Update();
}

void Shooter::Print()
{
	printf(
		"Shooter RPS: %4d Target RPS: %4d PID Output: %4.4f PWM Signal: %4.4f\n", 
		(int)irbreak->PIDGet(), 
		(int)flywheelPID->GetSetpoint(), 
		flywheelPID->Get(), 
		flywheel->Get()
	);
}
